//
// Created by JYL on 2023-11-28.
//
#include "balance_task.h"
#include "filter_task.h"
#include "mpu6050.h"
#include "cmsis_os.h"
#include "encoder.h"
#include "tim.h"
#include "pid.h"
#include "voltage_task.h"
#include "app.h"

BALANCE balance;
extern osSemaphoreId_t myBinarySem_carHandle;

/**************************************************************************
Function: Control function
函数功能：所有的控制代码都在这里面，5ms外部中断由MPU6050的INT引脚触发，严格保证采样和数据处理的时间同步
入口参数：无
返回  值：无
**************************************************************************/
void Balance_Task(void *argument)
{
    int Balance_Pwm=0,Velocity_Pwm=0,Turn_Pwm=0;		  					//平衡环PWM变量，速度环PWM变量，转向环PWM变
    /* Infinite loop */
    for(;;)
    {
        Application(app.state);
        //不需要等待陀螺仪采集滤波之后通知其来执行，这样会造成速度环采集不灵敏，速度环的PID可以再调节的
        //获取速度(m/s)、位移(m)
        Encoder_Left=Read_Encoder(4);
        Encoder_Right=-Read_Encoder(8);
        Get_Velocity_Form_Encoder(Encoder_Left,Encoder_Right);//编码器读数转速度（mm/s）
        //判断巡线中值，根据不同模式闪烁LED

        /*（直立，速度，转向）串级PID三环PWM计算*/
        //平衡PID控制 Gyro_Balance平衡角速度极性：前倾为正，后倾为负
        Balance_Pwm=Balance(mpu6050.Angle_Balance,mpu6050.Gyro_Balance);
        //速度环PID控制	记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
        Velocity_Pwm=Velocity(Encoder_Left,Encoder_Right);
        //转向环PID控制
        if(app.state==Line)Turn_Pwm=CCD_turn(balance.Middle_line,mpu6050.Gyro_Turn);//CCD循迹下的转向环控制
        else Turn_Pwm=Turn(mpu6050.Gyro_Turn);//

        //printf("%d,%d\r\n",balance.Middle_line,Turn_Pwm);
        //计算左/右轮电机最终PWM，限幅
        Motor_Left=Balance_Pwm+Velocity_Pwm+Turn_Pwm;       //计算左轮电机最终PWM
        Motor_Right=Balance_Pwm+Velocity_Pwm-Turn_Pwm;      //计算右轮电机最终PWM
        Motor_Left=PWM_Limit(Motor_Left,6900,-6900);
        Motor_Right=PWM_Limit(Motor_Right,6900,-6900);		  	//PWM限幅
        //检查是否小车被拿起//如果被拿起就关闭电机
        if(Pick_Up(mpu6050.Acceleration_Z,mpu6050.Angle_Balance,Encoder_Left,Encoder_Right))balance.Pick_up_stop=1;
        //检查是否小车被放下//如果被放下就启动电机
        if(Put_Down(mpu6050.Angle_Balance,Encoder_Left,Encoder_Right))balance.Pick_up_stop=0;
        //如果不存在异常//赋值给PWM寄存器
        if(Turn_Off(mpu6050.Angle_Balance,adc.battery_voltage)==0)Set_Pwm(Motor_Left,Motor_Right);
        //osDelay(5);//LED2_TOGGLE();
    }
}




/**************************************************************************
函数功能：异常关闭电机
入口参数：angle：小车倾角；voltage：电压
返回  值：1：异常  0：正常
**************************************************************************/
_Bool Turn_Off(float angle, float voltage)
{
    balance.Flag_Stop = MOTOR_EN();
    if(balance.Flag_Stop==1) balance.Pick_up_stop=0;//key2关闭，Pick_up_stop恢复为0
    //电池电压低于11.1V|倾角大于40度|Flag_Stop置1|Pick_up_stop置1，即小车基本静止，在0度左右拿起小车
    if(angle<-40||angle>40||1==balance.Flag_Stop||voltage<LOW_BATTER_VOLTAGE||MOTOR_EN()==1){
        PWMA_IN1=PWMA_IN2=PWMB_IN1=PWMB_IN2=0;
        return 1;
    }
    return 0;
}

int myabs(int a)
{
    int temp;
    if(a<0)  temp=-a;
    else temp=a;
    return temp;
}


/**************************************************************************
函数功能：检测小车是否被拿起
入口参数：Acceleration：z轴加速度；Angle：平衡的角度；encoder_left：左编码器计数；encoder_right：右编码器计数
返回  值：1:小车被拿起  0：小车未被拿起
**************************************************************************/
int Pick_Up(float Acceleration,float Angle,int encoder_left,int encoder_right)
{
    static u16 flag,count0,count1,count2;
    if(flag==0){                                                      //第一步
        if(myabs(encoder_left)+myabs(encoder_right)<300)count0++;//条件1，小车接近静止
        else count0=0;
        if(count0>10)flag=1,count0=0;
    }else if(flag==1){                                                      //进入第二步
        if(++count1>200)count1=flag=0;                       //超时不再等待2000ms，返回第一步
        if(Acceleration>30000&&(Angle>(-20+balance.Middle_angle))&&(Angle<(20+balance.Middle_angle)))   //条件2，小车是在0度附近被拿起
            flag=2;
    }else if(flag == 2){
        if(++count2>100)count2=flag=0;                    //超时不再等待1000ms
        if(myabs(encoder_left+encoder_right)>3000)                   //条件3，小车的轮胎因为正反馈达到最大的转速
        {
            flag=0;
            return 1;                                                //检测到小车被拿起
        }
    }
    return 0;
}
/**************************************************************************
函数功能：检测小车是否被放下
入口参数：平衡角度；左编码器读数；右编码器读数
返回  值：1：小车放下   0：小车未放下
**************************************************************************/
int Put_Down(float Angle,int encoder_left,int encoder_right)
{
    static u16 flag,count;
    if(balance.Pick_up_stop==0)return 0;//防止误检
    if(flag==0){
        if(Angle>(-10+balance.Middle_angle)&&Angle<(10+balance.Middle_angle)&&encoder_left==0&&encoder_right==0) //条件1，小车是在0度附近的
            flag=1;
    }else if(flag==1){
        if(++count>50)count=flag=0;//超时不再等待 500ms
        if(encoder_left>3&&encoder_right>3&&encoder_left<100&&encoder_right<100){ //条件2，小车的轮胎在未上电的时候被人为转动
            flag=0;
            return 1;                        //检测到小车被放下
        }
    }
    return 0;
}
/**************************************************************************
Function: Encoder reading is converted to speed (mm/s)
函数功能：编码器读数转换为速度（mm/s）
**************************************************************************/
void Get_Velocity_Form_Encoder(int encoder_left,int encoder_right)
{
    float Rotation_Speed_L,Rotation_Speed_R;						//电机转速  转速=编码器读数（5ms每次）*读取频率/倍频数/减速比/编码器精度
    Rotation_Speed_L = encoder_left*Control_Frequency/EncoderMultiples/Reduction_Ratio/Encoder_precision;
    balance.Velocity_Left = Rotation_Speed_L*PI*Diameter_67;		//求出编码器速度=转速*周长
    Rotation_Speed_R = encoder_right*Control_Frequency/EncoderMultiples/Reduction_Ratio/Encoder_precision;
    balance.Velocity_Right = Rotation_Speed_R*PI*Diameter_67;		//求出编码器速度=转速*周长
}

